/*********************************************************************
* File: mathLibrary.cpp
*
* \brief Miscellaneous mathematical functions.
*
* \author Instituto Superior Tecnico de Lisboa, Portugal
* \author Technical University of Lisbon, Portugal
* 
* \author Agentes Autonomos e Sistemas Multiagente
* \author Autonomous Agents and Multiagent Systems
* 
* \version	1.0
* \date		2006/2007
*********************************************************************/
#include "mathLibrary.h"
using namespace util;

/** 
* \brief Calculate a vector's squared length
*
* \param[in] vec The vector.
**/
float util::math::lengthSquared(const Vector &vec)
{
   // squared length, no sqrt involved so faster
   return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z;
}

/** 
* \brief This function adds or substracts 360 enough times needed to the given angle in
* order to set it into the range [0, 360) and returns the resulting angle. 
* 
* Letting the engine have a hand on angles that are outside these bounds may cause the
* game to freeze by screwing up the engine math code. 
**/
float util::math::angleMod(float a)
{
	return (360.0 / 65536) * ((int)(a * (65536.0 / 360.0)) & 65535);
}

/** 
* \brief This function adds or substracts 360 enough times needed to the given angle in
* order to set it into the range [-180, 180) and returns the resulting angle. 
* 
* Letting the engine have a hand on angles that are outside these bounds may cause the game
* to freeze by screwing up the engine math code.
*
* \param[in] angle The angle.
**/
float util::math::angleNormalize(float angle)
{
	return (360.0 / 65536) * ((int)((angle + 180) * (65536.0 / 360.0)) & 65535) - 180;
}

/** 
* \brief Normalizes an angle vector.
*
* \param[in] angle The angle vector.
**/
void util::math::clampAngles(Vector &vecAngles)
{
   vecAngles.x = util::math::angleNormalize(vecAngles.x);
   vecAngles.y = util::math::angleNormalize(vecAngles.y);
   vecAngles.z = 0;
}

/** 
* \brief Calculates the sin and cos of an angle.
*
* \param[in] rad The angle.
* \param[in] flSin The sin.
* \param[in] flCos The cos.
**/
void util::math::sinCos( float rad, float *flSin, float *flCos )
{
#ifdef __GNUC__
   register double __cosr, __sinr;
   __asm __volatile__ ("fsincos" : "=t" (__cosr), "=u" (__sinr) : "0" (rad));
   *flSin = __sinr;
   *flCos = __cosr;
#else
#ifndef _MSC_VER
   *flSin = sin(rad);
   *flCos = cos(rad);
#else
   __asm
   {
      fld DWORD PTR[rad]
      fsincos
      mov edx, DWORD PTR[flCos]
      mov eax, DWORD PTR[flSin]
      fstp DWORD PTR[edx]
      fstp DWORD PTR[eax]
   }
#endif
#endif
}

/** 
* \brief Calculates the normalized angle difference.
*
* \param[in] destAngle The destination angle.
* \param[in] srcAngle The source angle.
**/
float util::math::angleDifference( float destAngle, float srcAngle )
{
   return util::math::angleNormalize(destAngle - srcAngle);
}

/** 
* \brief Converts a spatial location determined by a vector into an 
* absolute Y angle (yaw) from the origin of the world.
*
* \param[in] vec The location.
**/
float util::math::vectorToYaw( const Vector &vec )
{
   if (vec.x == 0.0 && vec.y == 0.0)
      return 0;
   else
      return atan2(vec.y, vec.x) * (180 / M_PI);
}

/** 
* \brief Converts a spatial location determined by a vector into 
* absolute angles from the origin of the world.
*
* \param[in] vec The location.
**/
Vector util::math::vectorToAngles( const Vector &vec )
{
   float yaw, pitch;

   if (vec.x == 0.0 && vec.y == 0.0)
   {
      yaw = 0.0;
      pitch = (vec.z > 0.0) ? 90 : 270;
   }
   else
   {
      yaw = atan2(vec.y, vec.x) * (180 / M_PI);
      pitch = atan2(vec.z, vec.Length2D()) * (180 / M_PI);
   }

   return Vector(pitch, yaw, 0);
}

/** 
* \brief Creates a forward, right and up normalized vectors from a vector.
*
* The angles are set into the variables 'gpGlobals->v_forward', 
* 'gpGlobals->v_right' and 'gpGlobals->v_up'.
*
* \param[in] vecAngles 
**/
void util::math::makeVectors( const Vector &vecAngles )
{
   float sp = 0, cp = 0, sy = 0, cy = 0, sr = 0, cr = 0;
   float angle = vecAngles.x * (M_PI / 180);
   util::math::sinCos(angle, &sp, &cp);
   angle = vecAngles.y * (M_PI / 180);
   util::math::sinCos(angle, &sy, &cy);
   angle = vecAngles.z * (M_PI / 180);
   util::math::sinCos(angle, &sr, &cr);

   gpGlobals->v_forward.x = cp * cy;
   gpGlobals->v_forward.y = cp * sy;
   gpGlobals->v_forward.z = -sp;
   gpGlobals->v_right.x = -sr * sp * cy + cr * sy;
   gpGlobals->v_right.y = -sr * sp * sy - cr * cy;
   gpGlobals->v_right.z = -sr * cp;
   gpGlobals->v_up.x = cr * sp * cy + sr * sy;
   gpGlobals->v_up.y = cr * sp * sy - sr * cy;
   gpGlobals->v_up.z = cr * cp;
}

/** 
* \brief Calculates the ceiling of a floating point number, i.e., 
* the smallest integer which is equal or larger.
*
* \param[in] value The floating point number.
**/
int util::math::ceiling(float value)
{
	int clampedValue = (int) value;
	if(clampedValue == value)
		return clampedValue;
	else
		return clampedValue + 1;
}

/** 
* \brief Compares two vectors.
* 
* Compares two vectors to a specified precision.
*
* \param[in] v1 Vector A.
* \param[in] v2 Vector B.
* \param[in] precision The precision.
**/
bool util::math::compareVectors(Vector v1, Vector v2, float precision)
{
	return (fabs(v1.x - v2.x) <= precision) &&
		(fabs(v1.y - v2.y) <= precision) &&
		(fabs(v1.z - v2.z) <= precision);
}